// --------------------------------------------------------------------------------------------------------------------
// <copyright file="GraphOptimizationPose2DAgent.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Navigation.Runtime.Localization
{
    using System;
    using System.Collections;
    using System.Collections.Generic;
    using System.IO;
    using System.Linq;
    using System.Runtime.Serialization;
    using Microsoft.Robotics.Hardware.DAC;
    using Microsoft.Robotics.Hardware.Runtime.DAC;
    using Microsoft.Robotics.IO;
    using Microsoft.Robotics.Navigation.Localization;
    using Microsoft.Robotics.Navigation.Motion;
    using Microsoft.Robotics.Navigation.Perception;
    using Microsoft.Robotics.Navigation.Runtime.Localization;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Numerics.GraphOptimization;
    using Microsoft.Robotics.Runtime;
    using Microsoft.Robotics.Tracing;

    /// <summary>
    /// Agent that receives HDP with 2D poses, optimizes based on loop closing constraints, and reproduces a map.
    /// </summary>
    /// <typeparam name="TObservation">Type of measurement</typeparam>
    [DataContract]
    public class GraphOptimizationPose2DAgent<TObservation> : ConsumerProducerAgent<HDPwithPoseMessage<TObservation>, GraphMessage>,
        ISubscriptionReceiver<LoopClosingMessage>,
        IMessagePublisher<SimpleAgentMessage<GlobalMap>>,
        IMessagePublisher<SLAM2DAgentMessage>,
        IMessagePublisher<CompletionMessage>
        where TObservation : IHorizontalDepthProfile 
    {
        /// <summary>
        /// Width of the map to construct
        /// </summary>
        [DataMember]
        private int width;

        /// <summary>
        /// Height of the map to construct
        /// </summary>
        [DataMember]
        private int height;

        /// <summary>
        /// Step at which to downsample
        /// </summary>
        [DataMember]
        private int downSampleStep;

        /// <summary>
        /// Starting pose in the map
        /// </summary>
        [DataMember]
        private Pose2D startPose;

        /// <summary>
        /// Name of the loop closing producer agent
        /// </summary>
        [DataMember]
        private string loopClosingProducerName;

        /// <summary>
        /// Name of the file to save the constructed map
        /// </summary>
        [DataMember]
        private string mapFile;

        /// <summary>
        /// Class that handles the construction of a graph for SLAM 2D
        /// </summary>
        private GraphSLAM2D<TObservation> graph;

        /// <summary>
        /// Publisher of a the reconstructed graph
        /// </summary>
        private IForwarder<SimpleAgentMessage<GlobalMap>> globalMapPublisher;

        /// <summary>
        /// Publishes the trajectory list
        /// </summary>
        private IForwarder<SLAM2DAgentMessage> trajectoryPublisher;

        /// <summary>
        /// Publishes a completion message
        /// </summary>
        private IForwarder<CompletionMessage> completionPublisher;

        /// <summary>
        /// The Loop closing producer
        /// </summary>
        private IAgentHost loopClosingProducer;

        /// <summary>
        /// List of loop closures, used for down sampling of graph
        /// </summary>
        private List<Pose2DEdge> loopClosures;

        /// <summary>
        /// List of timestamps
        /// </summary>
        private List<long> timestamps;

        /// <summary>
        /// Reconstructed global map
        /// </summary>
        private GlobalMap map;

        /// <summary>
        /// List of loop closing messages
        /// </summary>
        private Queue<LoopClosingMessage> loopClosingMessages;

        /// <summary>
        /// Initializes a new instance of the <see cref="GraphOptimizationPose2DAgent{TObservation}"/> class
        /// </summary>
        /// <param name="name">name of the agent</param>
        /// <param name="hdpWithPoseProducer">the producer of $HDPwithPoseMessage$</param>
        /// <param name="loopClosingProducer">the loop closing producer</param>
        /// <param name="width">The width of the map to create</param>
        /// <param name="height">The height of the map to create</param>
        /// <param name="startPose">The starting pose in the map</param>
        /// <param name="mapFile">Name of the file to save the map</param>
        /// <param name="downSampleStep">Downsample size</param>
        public GraphOptimizationPose2DAgent(
            string name,
            IProducer<HDPwithPoseMessage<TObservation>> hdpWithPoseProducer,
            IProducer<LoopClosingMessage> loopClosingProducer,
            int width,
            int height,
            Pose2D startPose,
            string mapFile = null,
            int downSampleStep = 20)
            : base(name, hdpWithPoseProducer.Name)
        {
            this.loopClosingProducerName = loopClosingProducer.Name;
            this.width = width;
            this.height = height;
            this.startPose = startPose;
            this.mapFile = mapFile;
            this.downSampleStep = downSampleStep;
        }

        /// <summary>
        /// Called when the agent is initialized
        /// </summary>
        /// <param name="locator">agent locator</param>
        public override void Initialize(AgentLocator locator)
        {
            base.Initialize(locator);
            this.loopClosingProducer = locator.GetAgent(this.loopClosingProducerName);
            this.loopClosingProducer.Subscribe<LoopClosingMessage>(this);
        }

        /// <summary>
        /// Called when the agent is activated
        /// </summary>
        public override void OnActivated()
        {
            base.OnActivated();
            this.graph = new GraphSLAM2D<TObservation>(); ////new Graph<Pose2D, Pose2DEdge>();
            this.loopClosures = new List<Pose2DEdge>();
            this.map = new GlobalMap(this.width, this.height, this.startPose);
            this.timestamps = new List<long>();
            this.loopClosingMessages = new Queue<LoopClosingMessage>();
            this.loopClosingProducer.Activate();
        }

        /// <summary>
        /// Called when the agent transitions from active to inactive.
        /// </summary>
        public override void OnDeactivated()
        {
            base.OnDeactivated();
            this.loopClosingProducer.Deactivate();

            if (this.mapFile != null)
            {
                Serializer.Save<GlobalMap>(this.mapFile, this.map);
            }
        }

        /// <summary>
        /// Called to store the forwarder for the given type we want to publish
        /// </summary>
        /// <param name="forwarder">The forwarder to use when publishing</param>
        public void InitializePublisher(IForwarder<SimpleAgentMessage<GlobalMap>> forwarder)
        {
            this.globalMapPublisher = forwarder;
        }

        /// <summary>
        /// Called to store the forwarder for the given type we want to publish
        /// </summary>
        /// <param name="forwarder">The forwarder to use when publishing</param>
        public void InitializePublisher(IForwarder<SLAM2DAgentMessage> forwarder)
        {
            this.trajectoryPublisher = forwarder;
        }

        /// <summary>
        /// Called to store the forwarder for the given type we want to publish
        /// </summary>
        /// <param name="forwarder">The forwarder to use when publishing</param>
        public void InitializePublisher(IForwarder<CompletionMessage> forwarder)
        {
            this.completionPublisher = forwarder;
        }

        /// <summary>
        /// Called when a loop closing message is received
        /// </summary>
        /// <param name="message">The loop closing message</param>
        public void Receive(LoopClosingMessage message)
        {
            this.loopClosingMessages.Enqueue(message);
            this.AttemptToProcessLoopClosure();
        }

        /// <summary>
        /// Called on the receiving of a $HDPwithPoseMessage$
        /// </summary>
        /// <param name="message">The message received</param>
        public override void Receive(HDPwithPoseMessage<TObservation> message)
        {
            this.timestamps.Add(message.OriginatingTime);
            this.graph.Add(message.HDP, message.Pose);

            // bug here that will add unoptimized poses + hdp's in the constructed map
            // after optimization.  Need to look into.  Removed for now since there is
            // only one loop closure and adding unoptimized poses onto the optimized graph 
            // is not needed for the time being.  Cause of the glitch seen in the most recent
            // optimization of building 115
            // Bug 2130
           //// this.map.Update(message.HDP, this.map.MetersToMapCells(message.Pose));
            
            GraphMessage graphMessage = new GraphMessage(this.graph.Graph.Nodes, this.graph.Graph.Edges);
            this.Publisher.Post(graphMessage, true);

            if (this.loopClosingMessages.Count > 0)
            {
                this.AttemptToProcessLoopClosure();
            }
        }

        /// <summary>
        /// Attempts to process the most recent loop closures
        /// </summary>
        private void AttemptToProcessLoopClosure()
        {
            // attempt to process the most recent loop closing message
            LoopClosingMessage mostRecentLoopClosure = this.loopClosingMessages.Peek();
            if (mostRecentLoopClosure.Constraint.IndexI < this.graph.Graph.Nodes.Count && mostRecentLoopClosure.Constraint.IndexJ < this.graph.Graph.Nodes.Count)
            {
                this.loopClosingMessages.Dequeue();
                if (mostRecentLoopClosure.AddConstraint)
                {
                    this.loopClosures.Add(mostRecentLoopClosure.Constraint);
                }

                if (mostRecentLoopClosure.Optimize)
                {
                    this.graph.Optimize(this.loopClosures, this.downSampleStep);

                    GraphMessage graphMessage = new GraphMessage(this.graph.Graph.Nodes, this.graph.Graph.Edges);
                    this.Publisher.Post(graphMessage, true);

                    // construct the map
                    this.ConstructMap();

                    // publish optimized trajectories
                    Console.Write("Publishing trajectories...");
                    Vector2 shift = new Vector2(this.graph.Graph.Nodes[0].X - this.map.MapCellsToMeters(this.startPose.X), this.graph.Graph.Nodes[0].Y - this.map.MapCellsToMeters(this.startPose.Y));
                    for (int i = 0; i < this.graph.Graph.Nodes.Count; i++)
                    {
                        Pose2D pose = new Pose2D(this.graph.Graph.Nodes[i].X - shift.X, this.graph.Graph.Nodes[i].Y - shift.Y, this.graph.Graph.Nodes[i].Heading);

                        SLAM2DAgentMessage slamMessage = new SLAM2DAgentMessage(
                            this.map,
                            pose,
                            new Pose2D[] { pose },
                            null,
                            this.timestamps[i]);
                        this.trajectoryPublisher.Post(slamMessage, true);
                    }

                    Console.WriteLine("Done");

                    // publish completeion message
                    this.completionPublisher.Post(new CompletionMessage(this.Name));
                }
            }
        }

        /// <summary>
        /// Creates a Global map from the graph
        /// </summary>
        private void ConstructMap()
        {
            Console.Write("Constructing map...");

            Vector2 shift = new Vector2(this.graph.Graph.Nodes[0].X - this.map.MapCellsToMeters(this.startPose.X), this.graph.Graph.Nodes[0].Y - this.map.MapCellsToMeters(this.startPose.Y));
            for (int i = 0; i < this.graph.Graph.Nodes.Count; i++)
            {
                Pose2D pose = new Pose2D(this.graph.Graph.Nodes[i].X - shift.X, this.graph.Graph.Nodes[i].Y - shift.Y, this.graph.Graph.Nodes[i].Heading);
                this.map.Update(this.graph.HDP[i], this.map.MetersToMapCells(pose));
            }

            Console.WriteLine("Done");
        }
    }
}
